ON THE SIMULATION OF MULTIBODY SYSTEMS WITH 
HOLONOMIC CONSTRAINTS 



JUKKA TUOMELA, TEIJO ARPONEN, AND VILLESAMULI NORMI 

Abstract. We use Lagrangian formalism and jet spaces to derive a computational 
model to simulate multibody dynamics with holonomic constraints. Our approach 
avoids the traditional problems of drift-off and spurious oscillations. Hence even 
long simulations remain physically relevant. We illustrate our method with several 
numerical examples. 



1. Introduction 

The equations of motion of rigid body systems were established in the 19th century; 
for example Appell [2] gives a very nice and complete classical treatment of the subject. 
In the precomputer age solving a differential equation meant algebraic manipulation of 
the system such that the solution could be explicitly obtained. Hence in p] a lot of space 
is devoted to various specific situations and/or coordinate systems in which a complete 
solution could be obtained. Of course explicit solutions are usually impossible to obtain 
and hence the attention shifted gradually to quahtative, i.e. geometric, analysis of the 
properties of the solution set. This point of view lead to a modern differential geometric 
formulation of classical mechanics which can be found in [3j. However, the techniques 
developed in ^ and [3j, while interesting in proper context, are not necessarily suitable 
or useful for numerical treatment of multibody systems. 

One of the earliest attempts at the numerical simulation of the multibody systems 
was Baumgarte's stabilization method [4]. Subsequently there has been quite much 
interest in this topic and in [Tj [7] [17] [19] many different formulations are described. 
However, in spite of the extensive literature on the subject the simulation of constrained 
multibody systems remains a challenging problem and it appears that no definitive 
solution to the problem has been found. 

The major difficulty in the simulations is how to respect the constraints in long 
simulations and at the same time to avoid spurious oscillations which occur quite often 
in various stabilization methods. In this paper we propose a new method, based on our 
earlier work in [22], [23] and [21], which addresses these issues. 

Our computational model considers differential equations in jet spaces. Hence the 
differential geometry is essential in our approach; nevertheless our formulation differs 
from the standard geometric model given in p3]. We use Lagrangian formalism to derive 
the equations of motion; this is more natural in jet space context than Hamiltonian 
formalism. Constraints and possible invariants (like conservation of energy) are taken 
into account by restricting the dynamics to an appropriate submanifold of a jet space. 
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As a consequence quite much of the computing time is spent on projecting intermediate 
points to this submanifold. On the other hand the drift off is completely avoided. Since 
there are no spurious stiffness or oscillations we can use an explicit method in time 
integration. We have adapted a well-known Runge-Kutta method by Dormand and 
Prince with the classical step size control to our context [TTj. 

We consider only holonomic constraints in this paper. In case of point masses these 
kind of systems were already used as examples in our earlier papers. However, in 
multibody dynamics a good numerical representation of the orientation of the rigid 
body is somewhat involved, and hence the generalization of our approach to this case 
required some work. Nonholonomic problems, while certainly interesting, are beyond 
the scope of the present article and we refer to |5], [6], [12], and [15] for more details 
on this subject. 

The structure of the article is as follows. In Section [2] we briefly recall some geometric 
notions which are needed in our computational model. In section [3] we discuss the 
relevant variational principles leading to a Lagrangian formulation of, and derive, the 
equations of motion for one rigid body with external forces acting on it. We believe it 
is best to explain the main ideas in this simple context because extension to many body 
case is then, due to the holonomicity of the constraints, just a matter of establishing 
an appropriate notation. 

In Section H] we extend everything to an arbitrary number of rigid bodies and intro- 
duce invariants associated to multibody systems. We also recall for completeness what 
happens in the planar case: the whole model becomes significantly simpler because 
the orientations are trivial in this case. In Section [5] we describe the actual numeri- 
cal subtasks which are needed in our implementation. The description is rather brief 
because we can use extensively algorithms which are explained in more detail in [23]. 
Then in Section [6] we present our numerical results and finally in Section [7| give some 
conclusions and indicate some directions for future work. 

Acknowledgements We are grateful for useful and motivating discussions with 
Aki Mikkola and Asko Rouvinen from the Laboratory of Mechatronics and Virtual 
Engineering, Lappeenranta University of Technology, Finland. 

2. Basic tools 

For more information on standard differential geometry we refer to [20] and on jets 
to [16]. Further details about calculus of variations can be found in [9]. Differential 
equations in the jet space context are discussed in [14], [18] and [2T] . 

All maps and manifolds are assumed to be smooth, i.e. infinitely differentiable. All 
analysis is local, hence various maps and manifolds need to be smooth or defined only 
in some appropriate subsets. To simplify the notation these subsets are not indicated. 

2.1. Manifolds and jets. The €th. differential of a map / : i-^ R.'^ is denoted by 
d^f and its value at p by d^fp. The subscript is sometimes omitted for simplicity, if the 
point p is clear from the context. Let M be a manifold. The tangent bundle of M is 
denoted by TM, and the tangent space at p G M by TpM. A distribution D is a map 
that associates to each point p G M a subspace Vp of TpM. 

Let M be a submanifold of M". The objects defined on M can be taken to be defined 
on without writing explicitly the inclusion map. The inner product in MJ^ is denoted 
by (-, ■) and the same notation will be used also for inner products in TpM and TpM" 
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as usual. We may regard TpM as a subspace of TpM". The orthogonal complement of 
TpM, the normal space, is denoted by NpM. 

Let 71 : £ B he a bundle and let ti'^ : Jq{S) B he the bundle of g-jets of £. In 
the sequel we will mainly consider the case where S is the trivial bundle £^ = R x 
with the projection vr : M x — »• M. The coordinates of Jq{S) (called jet coordinates) 
are denoted hy {t,y\..., y", yl,...,y^). 

A section of the bundle is a map y : B S such that vr o y = id. In case of the 
trivial bundle the section is simply the graph of the map. In jet geometry it is usually 
more convenient to use sections than maps. For simplicity of notation we will use the 
same letter for maps and sections, the intended meaning being clear from the context. 

2.2. Some matrix groups and algebras. The special orthogonal group is 

§0(n) = {Ae M"^" I A^A = I , det{A) = l}. 

Let us introduce a convenient representation of elements of SO(3). We need this in 
order to define the orientation of the rigid body. This representation is discussed in 
detail in \Xh\ where it is derived using quaternions. First let 5*^ c be the unit sphere 
and let 9 & S^. Then we set 



R =HH 

(2.1) 




(9^ + 



It is now straightforward to check that R G SO(3). The parameters 9^ are called Euler 
parametersEI Let us note that 

- the rows of H and H are an orthonormal basis of TqS^, i.e. HH^ = HH^ = I. 

- the parameters 9 are not coordinates of SO (3) in the differential geometric sense. 

- 9 and —9 correspond to the same element R. Hence is a two sheeted covering 
space of §0(3) and consequently SO(3) is diffeomorphic to real projective space 
Rp3. 

Let us list some properties of H and H which are needed later. It is sometimes conve- 
nient to regard H and H as linear maps M'^ — > R^^'^. In this way it is natural to write 
Hi = H{9i). The following formulas are easily verified by direct computation. 

Lemma 2.1. 

H'^H = H^H = 1-99'^ 
HiH^ - HHf = HiH^ - HHf = 
HiH^ + HH^ = HiH^ + HH^ = 0. 
Moreover if v, w E are any vectors, then 

H{v)w + H{w)v = H{y)w + H{w)v = 0. 



""^a-lca. Rodrigues or Cayley-Klein parameters. 
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In particular 

He = He = HiOi = HiOi = 0. 

From the differential geometric point of view §0(n) is a Lie group, and the corre- 
sponding Lie algebra is 

so(n) = {Ae M"^" = -A}. 

Geometrically we may identify so{n) with T/§0(n). The important point for us is 
that so (3) and are naturally isomorphic as Lie algebras. To see this note that any 
Cl G so (3) is of the form 

/ -Q'^ 
n = i n'^ -n^ 
\-n^ 

Hence putting Q = fi^, we have for any f G 

(2.2) Civ = nxv. 

One consequence of this isomorphism is that the tangent bundle of §0(3) is trivial: 

T§0(3) ^ §0(3) X Ml 

From this it follows that the tangent bundle of is also trivial: TS^ ~ S"^ x M^. This 
is very important from the computational point of view as we will see later. 

2.3. Variational problems. Let (M, G) be an n-dimensional Riemannian manifold. 
We denote the coordinates of M by y. By the standard abuse of notation the same 
letter is also used for the curve y : M. ^ M and its coordinate representation. Then 
let us consider the variational problem where we want to find the extremals of the 
following map. 

(2.3) J{y) = J L{t,y,y,)dt 

where the integration interval, as well as the values of y at the end points, are fixed 
during the variation. The integrand is called the Lagrangian. 

Definition 2.1. The Euler-Lagrange operator and the Euler-Lagrange equations 
for the problem (12. 3p are 

^ , , d dL dL 

Extremals of (12.31) are solutions of the Euler-Lagrange equations. 
Then given a Riemannian metric G we may consider 

J{y) = \ j {yi,Gyi)dt. 

The extremals of this problem are called geodesies. Let us recall the Euler-Lagrange 
equations for geodesies. First we set 
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These are called the Christoffel symbols of the first kind. To write down the equations 
in a compact way we now introduce some nonstandard notation. Let us define the 
matrices 

Note that the matrices Chr^ are symmetric. Putting all these matrices together we 
obtain a "three dimensional" object: 

Chr = (Chri, . . . , Chr„) G M"X"X" Chr.^fc = [i j, k]. 

Note that Chr is not a tensor: it transforms differently in coordinate changes. Anyway 
we can now view Chr as a bilinear map: given f , w G M" we can now set 

Chr(f , w) = (^{v, Chriw), . . . , {v, Chr„w)j. 

Note that Chr{v,w) = Chr{w,v). Now a curve y : M — > M is a geodesic, if 

(2.5) G7/2 + Chr(yi,yi) = 0. 

In differential geometry books the Christoffel symbols of the second kind are more 
often used than the symbols of the first kind. The symbols of the second kind are the 
components of the "matrix" G~^Chr. However, in computations one wants to avoid 
inverting matrices because this is both inefficient and unnecessary. 

The equations for geodesies can be formulated in a coordinate free way using the 
(Levi-Civita) connection. From this point of view the Christoffel symbols represent the 
connection in a coordinate system. 

2.4. Differential equations. Let vr : £ ^ B he a bundle. 

Definition 2.2. A (partial) differential equation of order q on S is a submanifold TZq 

of MS). 

In jet coordinates the manifold TZq can be represented as a zero set of some map 
/ : Jq{S) - M('?+i)"+i ^ M'^: 

(2.6) TZq : f{t,y,y,,...,yq)=0. 
To define solutions we introduce the following one forms 

(2.7) a] = dy]_^ - y]dt i = 1, . . . , n j = 1, . . . , g. 

Let p G Jq{£) and Vp G TpJq{£) and let us further define distributions C and V by 

Cp={vpeTpJq{S)\aUvp) =0}, 

(2.8) 

T)p =TpTZq n Cp, 

C is called Cartan distribution. Now we can define the solutions as follows. 

Definition 2.3. Let TZq C Jq{S) be involutive and suppose that the distribution T) 
defined in (12. 8p is one-dimensional. A solution of TZq is an integral manifold ofT>. 

The importance of involutivity from the point of view of numerical computations is 
discussed in [22j. Intuitively a system is involutive if we cannot get new equations of 
order q or less by differentiating the equations and eliminating the higher derivatives. 

Now the equations of motion in Lagrangian mechanics take a very particular form 
and it turns out that using directly the above formulation of the problem would be 
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very inefficient. Fortunately it is not difficult to adapt the jet space approach to this 
class of problems as we will see. Let us here briefly indicate what kind of changes we 
have to make to the general framework. 

As is well known the Lagrangian dynamics with holonomic constraints gives a system 
of second order equations which can be written as 

(2.9) hit,y,yi,y2,X) = 

where y : M ^ gives the coordinates of the configuration space and A : M — > is 
the Lagrange multiplier. Now if we regard A simply as any other dependent variable, 
then we would have to work with the space J2{£\) where £x = M.x M" x and whose 
dimension is 

dim(j2(^A)) = 3(n + £) + L 

Moreover the system (12. 9p is not in the involutive form; in fact usually one has to 
differentiate and eliminate 4 times before we reach the involutive form, see [22j for an 
example. However, it is unnecessary to treat y and A in the same way, and we can work 
with the space Ji{£) whose dimension is just 

dim(Ji(^)) = 2n + 1. 

So our problem is geometrically as follows: 

• the set of possible states of the system is given by a manifold M C Ji{£) 

• the dynamics of the system is given by a one dimensional distribution V on M 

Before a detailed description of the computational model let us here outline the basic 
strategy. First the manifold M is given as a zeroset of some map, and solutions are 
curves on M. To choose the right curve through some point p we need to compute 
the appropriate distribution at p. Now given p = (t, y, yi) G M it is easy to see that 
Vp = {l,yi,w) G Cp for any w. Hence our task is to compute the correct w. Next 
consider the section j^{y) : M — » M C Ji{£) given by t (t,y{t),y'{t)); the tangent 
vector to this curve is {l,y'{t),y"(t)), and hence it seems that we should take w = ?/2- 
However, y2 is "outside" of Ji{S), so this is not directly applicable. But then we show 
that we can compute the correct y2 with help of the Euler-Lagrange equations, and 
use this to define the right distribution. 

3. One rigid body 

It turns out that all the relevant ideas that we will need in our computational model 
can be explained already in case of one rigid body, and extension to the general case 
is mainly a matter of introducing appropriate notation. Hence we feel that one gets a 
clearer picture of our approach by treating thoroughly the simple case and then rapidly 
indicating how to pass to the general case. 

Equations of motion for rigid bodies can be found in most textbooks on classical 
mechanics. However, many formulations are not at all suitable for numerical compu- 
tations. There are also many books which are devoted to computational aspects of 
multibody systems, see for example [1] [7] [15] [17] [19]. Since our model is new we 
cannot, however, always refer to standard literature for details, and hence we have to 
spend some time to describe our approach. In particular as far as we know jet spaces 
have not been previously used in multibody simulations. 
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3.1. Configuration space and state space. To describe the motion of a rigid body 
we need a fixed coordinate system (or spatial coordinate system), and the coordinate 
system moving with the body (or body coordinate system). The origin of the moving 
coordinate system is assumed to be at the centre of mass of the body. A typical point 
in spatial coordinates is denoted by x and in body coordinates by x- The body itself 
is denoted hy B C. M'^, its mass density by p and its mass by m = m{B). 

Definition 3.1. Let t> G M'^ and set 

Iv = / (x X (t; X x))p{x)dx 
Jb 

I is the inertia tensor of body B. 

One can readily check that I is symmetric and positive definite. The position of the 
centre of mass in the fixed coordinate system is given the map r = (r^, r^, r^) : M — > R^. 
To describe the motion of other points of the body we need to specify the orientation 
of the rigid body; this can be done with a rotation matrix. Now the relation between 
spatial and body coordinates is given by the formula 

x{t) = r{t) + R{t)x 

where R{t) G §0(3). Hence the configuration space of one rigid body is R'^ x SO(3). 
However, it is computationally not easy to work directly with the space SO(3). Instead 
we will choose the following framework. 

Definition 3.2. The configuration (resp. state) space of one rigid body is the bundle 
Q = RxR'^xS^^R ( resp. Ji(Q) ). 

Geometrically this means that we replace SO(3) by its covering space S*^, using 
the representation given by (12.11) . Recall that in jet context it is natural to think in 
terms of bundles; classically one thinks in terms of fibers, and hence the state space is 
the tangent bundle of the configuration space. This is really almost the same as our 
formulation because of the following identifications: 

Ji(Q) ^ R X T{M? X ~ R X TM? x TS^. 

Let us then introduce the bundle 

7r:^ = RxR^->R y = [y^ ^ . . . ^y^) = {r,e). 

In this way we consider Q as a subset of £. Then we define 

/, : Ji(^)^R2 fe{t,y,y,) = 

Me = f-\0)CMS). 

Definition 3.3. Mg is the computational state space of one rigid body. 

This representation of the state space is much more convenient than the represen- 
tation obtained by introducing coordinates on or SO (3). Neither of these spaces is 
diffeomorphic to R'^, hence any coordinate system is necessarily local. So in general one 
should change coordinates during the computation, and this would be very annoying. 
On the other hand the parameters 6, while not coordinates, provide anyway a global 
representation of the configuration space. 




8 



JUKKA TUOMELA, TEIJO ARPONEN, AND VILLESAMULI NORMI 



Finally let us note that Mg, being a submanifold of Ji{S), is a differential equation 
according to Definition 12.21 However, it is obviously underdetermined: the dimension 
of the distribution defined in (I2.8P is greater than one. Note that dim(Cp) = 8 and it 
is straightforward to check that 

dim (TpMe n Cp) = 7. 

This dimension reflects in a natural way the degree of indeterminacy of the system: 
there are 6 degrees of freedom and the time variable. 

So we have now an appropriate state space for the computational purposes. The 
next task is to introduce dynamics; i.e. choose an appropriate distribution on Mg. 

3.2. Variational principle. The equations of motion of mechanical systems are usu- 
ally derived from some variational principle. There are many essentially equivalent 
principles, see for example [2, Tome 2, p. 451 and p. 492], pi Chapter 3] and P, p. 
205] for some discussion. In addition one has to choose between Hamiltonian and La- 
grangian formalism. We use the latter because it is more natural in jet space context. 
The formulations of variational principles below are adapted from [6l Chapter 4]. The 
statements are given geometrically, i.e. in a coordinate free way. Expressing them in a 
coordinate system give formulas which are found in classical textbooks. 

We will use a variational principle to determine the distribution on Mg. To do this 
we first formulate the variational principle in Ji{Q) and then interprete the result in 
terms of Mg C Ji{S)- The starting point is the kinetic energy of the rigid body. To 
define this we need to introduce angular velocities. 

Let R : §0(3) and define 

Cl = Ri uj = RiR^ . 

These matrices belong to so (3); hence we can define corresponding Vt and uo as in (12.21) . 
VL and f2 (resp. Co and uj) is called the body (resp. spatial) angular velocity. 

Definition 3.4. The kinetic energy of a rigid body is given by 

(3.1) T = rt, + r,o = |m|rl|2 + i(^],m) 

where Ttr is the translational energy and T^o the rotational energy. 

The angular velocities are related to 6i by the following simple formulas. 

Lemma 3.1. 

n = 2Hei uj = Rn = 2H9i. 

Proof. Using Lemma [2. II we compute 

Cl = R^Ri = HH^{HiH^ + HH^) = 2HHf 

and it is straightforward to check that H9i = HH^, proving the claim for il. Similar 
computation proves the formula for u. □ 

Hence the kinetic energy can be written as 

T = ^m\ri\'^ + 2{Hei,IHei). 

Now the variational principl^ we need is: 



^a.k.a. the Jacobi principle [T3] 
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The kinetic energy defines a Riemannian metric on the configuration space. The 
motions of the rigid body in the absence of forces are geodesies for this metric. 

The technical difficulty in using this principle is related to the rotational energy, so let 
us first treat the case that the rigid body is fixed at the centre of mass. The problem is 
that we cannot directly use the equations ( 12.4P or formulas ( 12.5P because the parameters 
9 are not coordinates on 5"^. Hence we have to introduce some local coordinates, and 
express the parameters 9 in terms of these coordinates. 

So let us choose a local parametrisation of S*^, i.e. we choose some open sets Ui C 
and U2 C and a diffeomorphism if : Ui ^ U2, 9 = if{a). Hence we can write the 
rotational kinetic energy as 

(3.2) Tro = 2 {Hd(p ai, IHdip ai) = | (ai, Grotti) 

where the Riemannian metric Gro is given by 

Gro = Mip'^H^IHdif 

Now we could use the Euler-Lagrange equations (12.41) or formula (12. 5p to compute 
the equations of motion in terms of a. However, we want to interprete the resulting 
equations in terms of 9. It seems that in this case it is best to start again from Euler- 
Lagrange equations, rather than trying to express Christoffel symbols in terms of 9. 

Hence we need to compute the Euler-Lagrange operator for the Lagrangian T^. First 
let us define 

dGUv, w) = ((., ^w), {v, ^w), {v, ^w)) 

Thus dGro is a symmetric bilinear map dG^ '■ K'^ x M'^ R'^. Next we establish some 
formulas which are needed in the computations. 

Lemma 3.2. 

c^V(«ir) = {j/^Y 
dH9i = -Hi dip 



Proof. The first formula is quite straightforward to check. Then let us denote by 
(resp. Bi) the ith column of the left (resp. right) hand side of the second formula. Then 
using Lemma [2TT] we compute 

A = = H{^)9i = ~H{9i)^ = -H,^ = -B. 



Lemma 3.3. The Euler-Lagrange operator for the rotational energy is 

(3.3) E^Ja) = - ^ = Ad^^H^lH92 + 8d^''H^m9i 

dt oai oa 



□ 
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Proof. First we compute 

d dTrn d 



dt dai dt ™ ^ 
4 dip^H^lH^ {dip ai) + 4 dif^H^lHi dip ar 



Adip'^HflHdipai + 4 [j^d^Y H^lHd^ai = 
Adi/H^lHOi + Adip'^H^IHei + 4 {^d^f H^lHOi. 



Further we obtain 



Hence we can write 

= IdGJauai) = 4d'^^(ai, ■)H^IH9i + 4(dH IH9i. 

da 

Then Lemma [3.21 gives the result. □ 

3.3. Forces and constraints. In practice the rigid body is not free, but there are some 
forces acting on it. First let us introduce potential forces. Recall that the potential 
is simply a function U in the configuration space. The variational principle can be 
extended to such systems in the following form: 

Motions of a rigid body fixed at the centre of mass subject to potential forces are 
given by the extremals of the Lagrangian 

L{a, tti) = Tro — U{a). 

Hence the computations in Lemma 13.31 readily yield 

Corollary 3.1. The Euler-Lagrange operator for L = — U is 

El (a) = d(^^(4 H'^IHe2 + 8 H^IHei + VU) . 

Next we have the effect of external forces. Since the body is fixed at one point, 
we need only need to consider the torque acting on the body. Now given the torque 
T in body coordinates, or = Rt in spatial coordinates, how to incorporate this 
information in Euler-Lagrange equations? The appropriate concept in our context is 
given in the following definition. For a more general formulation we refer to ^6, p. 188]. 

Definition 3.5. F^- is Lagrangian torque corresponding to t, if 

{Fr,ai) = {ts,uj) 

for all «!. 

The next version of the variational principle is as follows. 

Lagrangian torques are added to the right hand side of Euler-Lagrange equa- 
tions. 

Then we have to compute Fr. 
Lemma 3.4. 

Fr = 2d^^H^T. 
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Proof. 

{Fr,ai) = (r„cu) = {RT,2m) 

= 2{H^HH^T,dipai) = 2{dip'^H^T,ai). 

□ 

Hence our system so far can be written as 

EL(a) = d(/?^(4 H^me2 + 8 HflHei + VU) = 2dip'^H^r. 

Let us then introduce holonomic constraints. We will suppose that the constraints are 
scleronomic, i.e. do not depend on time. Hence the constraints are given by the map 

where Ui is the domain of ip. Let us also set ga = 9 oip. Now the system is required to 
stay in the subset of the configuration space defined by the zero set of ga'- 

= (7-1(0) Cf/i. 

To force the system to stay in the correct manifold the variational principle is modified 
as follows: 

In the presence of holonomic constraints a (fictions) constraint force Fc, which 
is normal to the constraint manifold, is added to the right hand side of Euler- 
Lagrange equations. 

Hence at present we have 

El (a) = dip'^{A H^me2 + 8 HflHei + VU) = 2dip^H^T + F^. 
Lemma 3.5. We have 

(3.4) 4 + Hdg^X + 8 HH^lHOi + HVU = 2r 

where A is the Lagrange multiplier. 

Proof. The rows of dga span NaM^. Hence 

EL(a) = dip'^{A H^IHe2 + 8 H^mOi + VU) = 2d^p^H^T - dg^X 
for some A. But then dga — dg d(fi implies 

di/^{4: H'^me2 + dg'^X + 8 H'^lHOi + VU - 2H'^t) = 0. 
Since the columns of dip span TgS^ this is equivalent to 

4 H^lHe2 + dg'^X + 8 H'^lHOi + VU - 2H^t e A^^^^ 
whence pre-multiplying by H gives the desired formula. □ 

For convenience let us give the following 
Definition 3.6. The bilinear map 

K {61,61)= HHIIH61 

is called the Christoffel map. 
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Note that K contains the information of the Christoffel symbols for the metric defined 
by G,o- 

Recall that our goal is to compute 62- The previous Lemma shows that we also have 
to compute A in the process. Both can be determined as follows. 

Algorithm 3.1. 

• given (t, 9, 9i) do 

- solve c and A from the following system 



(3.5) 



4 Ic + Hdg^X = 2t-8K - HVU 
dgH^c= \9i\^dg9 - d^g{9i,9i) 



- set 92 = H'c-\9i\'9 



Proof. Since 6*2 G ~ TgS^ © NgS^ we have the corresponding decomposition 

92 = H^c + a9 

for some c and a. Differentiating the constraint — 1 = twice it is seen that 
a = — l^^ip. Similarly differentiating the constraint g twice we get 

dg92 + d^g{9^,9,)=0 

But then substituting the decomposition of 92 to this equation (resp. to (13. 4p ) gives 
the second (resp. first) equation in (13. 5p . □ 

Now that the hard work has been done the rest follows rather painlessly. If the centre 
of mass of the rigid body is not fixed, then the relevant Riemannian metric is 

fmh 

Computing the appropriate Euler-Lagrange operator is straightforward. Then we have 
to also include the resultant of the external forces, denoted by F, which is applied at 
the centre of mass. Further we set d = (r2,c), Fe = {F,2t) (where r is the torque in 
body coordinates as before) and 



H 



/m/3 \ ^ _ / 

VO '^-[Ki9„9,: 

Next we introduce constraints. Let g : £^ — > be a map which does not depend on 
time and set 

Rx Mg = g-\0) C S. 

Further we define 

"dg yi 



/he : M£)^m'' fucit,y,y,) = 

Mhc = /h-'(0)c Ji(^) 
where the subscript he refers to "holonomic constraints". 
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Definition 3.7. The constrained configuration and state spaces are 

Qco = Q n (M X Mg) C £ 
M = MenMhcC Ji{£) 



Now with these notations we can compute the relevant Euler-Lagrange equations in 
very much the same way as above. Since this extension is routine we merely state the 
final result. 

Theorem 3.1. Let us consider the motion of a rigid body with the following data: 

- Lagrangian is L = T — U where T is given by ( 13. ip and U is the potential. 

- The body is subject to force = {F, 2r) . 

- Constrained state space is given as in Definition \3.7 . 



(3.6) 



Then the following algorithm computes the distribution on M. 

Algorithm 3.2. 

• given p = {t,y,yi) G M do 

- solve {d, A) from the following system 

'Ed+ Hdg^X = F,-8K- HVf/ 
dg H'^d = dg\y- d'^g{yi, yi) 

- set y2 = H^d — \y. 

- set Vp = span{(l,yi,?/2)}- 

We summarize our conclusions as follows: 

Algorithm \3.2\ determines a one dimensional distribution on M. The integral 
manifolds of this distribution give the motions of one rigid body. 

Traditionally one speaks about equations of motion for the rigid body. In this way we 
could say that the description of M and V are our equations of motion. Note that in 
this model an integral manifold can always be represented by a curve y : M ^ Qco- 
This is due to the first component of the distribution being always positive. Hence we 
will refer to the solutions as curves whenever convenient. 

3.4. Energy. Finally we examine how the energy W = T + U evolves. 

Lemma 3.6. If y is a solution, then 

^ = (Hyi, Fe) = (ri, F) + 2{He,, r) = {r,, F) + (fi, r) 
dt 

Proof. The energy can be written as 

W = l{Hy,,EHy,) + U{y) 

Hence 

^ (Hyi, E Hy^) + (Hyi, E Hm) + (Vf/, yi) 



dt 

= (Hyi, E Hy2) + (Vf/, yi) = (yi, H^E d) + (Vf/, yi) 



14 
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because Hi?/i 



{O^HiOi) = (0,0). Then using (JHH) we obtain 



Now the result 



follows from the following computations. 



(yi, H^Hd^7^A) = {yi^dg^X) = {dgy,,\) = 
(l/i,H^HVf/) = (yi,Vf/) 

{yi, H^K) = {ei,H^HHiIHei) = {Hi6i,lH6i) = 



□ 



3.5. Classical formulation. Classically the equations of motion for the rigid body 
are often written as follows. Let F be the resultant of the external forces and let r be 
the torque acting on the body. Then the equations of motion can be written as 



The first equation is simply the Newton's second law, and the second equation is 
sometimes called Euler's equation, and hence the whole system is called Newton-Euler 
equations. Appell [21 p. 48] refers to this system combined with the energy balance 
equation as sept equations universelles. 

This way of viewing things is not so convenient when we have a system of several 
interacting bodies with constraints. One reason is that in Newton-Euler equations one 
has to consider internal as well as external forces. On the other hand in the variational 
formulation only external forces appear. 

The fact that the first equation is of second order while the second one is of first order 
already indicates that the variables r and VL are not "on the same level". Physically 
this is clear since r describes position while Vt describes (angular) velocity. In any case 
it would be hard to avoid the conclusion that the Newton-Euler system is not very 
suitable for numerical computations, see [15] for a detailed discussion. 



4.1. Extension to general case. Let us now formulate the problem with arbitrary 
number of rigid bodies. Let the number of bodies be and let the configuration space 
be 



(3.7) 




4. Systems of Rigid bodies 



Q = M X 




The coordinates of 8 are 
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where (r^^\9^^^) denote the position and Euler parameters of the ith body. The com- 
putational state space Mg C Ji{S) is the zero set of the map 

(4.1) /, : Ji(^)^M2n, fe{t,y,y,)= : 

|^K)|2_ 1 

Let us further define 





E = 


diag(EW,. 




1 = diag(|(^), 


. . , |("'')) 


(4.2) 


H = 


diag(HW,. 










d = 











where etc denotes the corresponding matrix or vector for the ith rigid body. The 
Lagrangian and the energy of the system can now be written as 

L = T-U=J2-2 + 2 {H^'^eP^I^'^H^'^e?) - U{y) 



= \{Hy^,EHy^)-U{y) 

W = T + U =\{Wy^,EHy^) + U{y). 

Next we introduce constraints in the same way as in the case of one rigid body, see 
Definition O We set 



(4.3) 



g : S^W 
/he : Ji{£) 



t,2£ 



fhc{t, y, yi) 



dg yi 
9 



Mhc = /h-'(o) C MS) 

Qco = Q n (M X Mg) M = Me n Mhc. 

Since we have i independent constraints one says that there are Gn^ — i degrees of 
freedom in the system. In our context this can be checked by computing that 

dim(TpM n Cp) =6nb-i + l. 

Recall that is because of the time variable. 

Then we get the following result. The proof is omitted because it is essentially the 
same as in the case of one rigid body. 

Theorem 4.1. Adopting the notations in (14. 2 p and (14. 3p . Alaorithm \3.S\ computes the 
distribution also in the case of several rigid bodies. Moreover we have 

lF = <"^-^->- 

Hence in absence of external forces the energy remains constant. Moreover the 
constraint forces have no effect on energy. This is sometimes expressed by saying that 
constraint forces do no work. 
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Note that the interconnection forces between different bodies do not appear in this 
formulation; we need only specify the external forces acting on the system. On the other 
hand if Newton-Euler equations are used, it is necessary to take care of interconnection 
forces as well. Hence it is by no means obvious that both models really yield the same 
results. However, a detailed discussion of the equivalence of variational and Newton- 
Euler models is beyond the scope of our article and we refer to [6], Chapter 4] for more 
information and references on this topic. 

Now if we have external forces acting on the system it is convenient to define Wext, 
the work done by external forces: 

Wext(t)= / (Hyi,Fe)rfs 

Jo 

In this way the total energy 

W^total = T + f/ - W^ext 

remains constant. This is useful in computations because we can compute W^xt approx- 
imatively using (for example) trapezoidal rule, and use this to control the total energy 
balance. 

4.2. Invariants. In addition to constraints, there may be invariant^ associated to 
the system which are modelled by a map /inv : ^i(^) ~^ While invariants and 
constraints might superficially seem similar concepts, in reality they are of very different 
nature. Constraints are externally imposed on the system. Mathematically this means 
that Lagrange multipliers are needed in the corresponding equations. Physically this 
means that we introduce fictions forces which realize constraints. Invariants on the 
other hand are logical consequences of the dynamics. Hence their description requires 
neither Lagrange multipliers nor fictions forces. 

A typical example of an invariant is the conservation of energy: in the absence of 
external forces we have seen that W is an invariant. Usually the number of invariants is 
quite small, and indeed in many cases the energy is the only invariant. The invariants 
define a manifold 

Mi,v = /i;J(o)c Ji(£:). 

Geometrically Minv H M can be interpreted as a submanifold of M which is invariant 
by the flow induced by the dynamics of the system. Hence the presence of invariants 
has no effect on the computation of the distribution. 

4.3. Planar case. Of course the planar case is included in the above considerations 
as a special case. However, treating planar case directly is much more efficient compu- 
tationally. Hence we briefly indicate the appropriate model. 

Now in this case the relation between spatial and body coordinates is given by 

x{t) = r{t) + R{t)x 

where R G SO(2). But using the correspondence 

/ cos(/5) - sin(/5)\ 
^ Vsin(/5) cos(/5) ) 

■^Invariants are also called constants of motion or sometimes even dynamical constraints. In the 
latter case our constraints are then kinematic constraints. 
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it is seen that the configuration space is x S^. Computationally we can regard 
/3 G M, with the understanding that the relevant data should be 27r periodic. Next, it 
is evident that 

uj = n = (0,0, A) 

and the inertia is described by the scalar 

= / \x\'^pix)dx- 

JB 

The torque is also a scalar: r ~ (0, 0, r). Then setting y = (r, /?), = (F, r), denoting 
the potential by U and defining 

mJ2 



E 



Ip 



we can write the Euler-Lagrange equations as 

Ey2 + VU = Fe. 

This formula can of course be generalised to the case of system of planar rigid bodies 
by following conventions: 

E = diag(F(^),...,F('^'')) 

Fe = {F^^\t\...,F^'"'\t'"'). 

Finally if the constraints are given by g we see that the relevant distribution can be 
computed from the following equations. 

Ey2 + dg'^X + VU = F^ 
dgy2 = -d'^g{yi,yi). 



(4.4) 



In particular it is seen that no term corresponding to Christoffel map appears in the 
planar case. The determination of the (constrained) state space is handled in the same 
way as in the general case. 

5. Computations 

The computational problem has 2 main ingredients: 

(i) given p G M, compute Vp 

(ii) given a G Ji{S), project a to M. 

Of course in the latter problem a must be sufficiently close to M so that the projection 
is well defined. However, this is not a problem in practice since this condition is always 
satisfied if the step size is sufficiently small. 

5.1. Distribution. How to solve the system (13.61) as efficiently as possible? The fol- 
lowing approach was already used in p3] where more details and references can be 
found. Consider the block matrix 

fA B^' 

C = 

\B D 

If A is invertible the Schur complement of A is 

S = D- BA-^B'^. 
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Now the Schur complement is useful in solving the linear system Cw = b ii 

- A is "easily invertible" and 

- D (and hence S) is (much) smaller than A. 

But this is precisely the situation in our case! The system (I3.6P can be written as 

E H{dgf\ ^d\ fFe-8k-HVU\ 
dgH^ )[x)~[dg\y-d'giyi,yi))' 
The Schur complement is now 

S = -dg H^E-^H{dgf. 

Typically the size of S is much smaller than the size of E. But even more importantly, 
E is indeed easily invertible, because it is block diagonal matrix with 6x6 blocks. 
Hence we solve the system (13.61) as follows: 

Algorithm 5.1. 

• solve Eu^ = Fe-8k- HVf/ 

• compute = dg H^m^ — dg\y + d'^g{yi, yi) 

• solve Sf^ = . 

• compute = H{dg)^v^ 

• solve Eu^ = v? 

• set {d, A) = {u^ + -v'^) . 

Note that the system Sw ^ = must be solved iteratively while E systems are solved 
"exactly" by a direct method. Then we get the following overall algorithm for the 
computation of the distribution. 

Algorithm 5.2. 

• given p = (x, y, yi) G M do 

- solve {d, A) from the system (13.61) using Algorithm 15.11 

- set y2 = H^d — \y 

- set Vp = span{(l,?/i,?/2)}- 

5.2. Projection. We have in fact 3 relevant manifolds which we have to consider: Mg, 
Mhc and M\„y. Let us first consider Mg. Since this involves only Euler parameters we 
formulate the projection directly in terms of 6. Moreover different rigid bodies do not 
interact in the projection, so without loss of generality we consider just one rigid body. 

In principle the orthogonal projection of (a, ai) G T^R^ to {0,9i) G TgS^ could be 
computed by solving the system 

'6 + fii9 + ^1201 = a 
(5.1) U+f^.e = a, 

However, this would require Newton's method. Instead we project with the following 
algorithm. 

Algorithm 5.3. 

• given (a,ai) G T^R^ do 



SIMULATION OF MULTIBODY SYSTEMS 



19 



(5.2) F(y,yi,a,/3) 



— set = a/\a\ 

— set 6i = ai — (ai, 6)6. 

Note that the point thus obtained satisfies 3 last equations of (15. ip . However, the 
first equation will be satisfied for some /zi only if (a, ai) = 0. Anyway in our application 
(a,ai) will always be "small", more precisely = 0{h), so we may say that the 

projection is "almost" orthogonal. 

Next let us consider Mhc. Recall that we suppose that the constraints do not depend 
on t. Then given a point (t,a,ai) G Ji{S), its orthogonal projection to (t,y,yi) G M^c 
can be computed by solving the following system. 

fy + d'^g^{yi, ■)a + — a\ 
yi + dg'^a — oi 
dgyi 

V 9{y) ) 

But now we can do an "almost orthogonal" projection mimicking Algorithm [5?3] because 
the form of the equations is essentially the same in both cases. Given (t, a, ai) G J\[S-^ 
we can orthogonally project a to ?/ G Mg by solving the system 

y + [dgY II — a = 

9{y) = 0. 

But having computed this we can solve yi and a from the system 

yi + {dgYa - ai = 
dgyi = 0. 

Note that this is a linear system. Then we have obtained values {y, yi, a) such that the 
last three equations of the system fl5.2p are satisfied. 

Algorithm 5.4. 

• given (t,a, ai) G Ji{S) do 

— project a to y E Mg by solving (15.31) 

— solve yi and a from system (15. 4p . 

Finally we have the invariants, given by the map /inv Now in general this map has 
no special structure, so given a = (t,a,ai) G Ji{S) we simply solve 



(5.3) 



(5.4) 



(5.5) 



P + {dfinvVjJ, - a = 

/inv(p)=0. 

All these projections are then combined as follows. 
Algorithm 5.5. 

• given pinit G Ji{S) do 

- set p° = Pinit, j = 

- repeat 

project p' to p0 G Mq using Algorithm 15.31 
project p0 to phc G Mhc using Algorithm 15.41 
project phc to p^^^ G Mjnv by solving the system (15. 5p 
until convergence. 
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5.3. Constraints in practice. In practice the constraints for rigid bodies are of quite 
particular form. In fact it turns out that there are only three basic constraints and all 
needed constraints can be constructed by taking appropriate combinations of the basic 
ones. 

The first one is a coincidence constraint (or briefly C - constraint) ^ given by a map g"^ : 
M}^ M^. This constraint simply says that given points x*-*"* and x^''^ in the coordinate 
systems of bodies Bi and Bj are really the same point in the spatial coordinate system. 
Hence 

(5.6) g^{Bi, Bj) = r(^) + - r» - R^^x^^ = 0. 

Mathematically x^*^ and x''"'^ can be arbitrary, but in practice they are the positions 
of the relevant joint in the corresponding body coordinate systems. We remind the 
reader that r^^^ is the position of the centre of mass of the body Bi and i?^*^ defines its 
orientation in the spatial (global) coordinate system. 

Next we introduce a basic constraint where we require that two unit vectors a^^\ a^^^ 
given in body coordinate systems must be perpendicular to each other in the spatial co- 
ordinate system. This is called symmetric orthogonality constraint (or SO -constraint), 
and is given by the following map g^° : ^ M: 

(5.7) (7'°(a^*\ a(^')) = {R^'^a^'^, R^^^a^^^) = 0. 

In the third constraint we are given a unit vector a^*-* and the points x^*^ and x*-"''* in 
body coordinates. Let us consider the difference of x*-*^ and x^^^ in spatial coordinates: 

Now we require that this is orthogonal to a*^*-* which must naturally also be expressed 
in the spatial coordinates. This is called nonsymmetric orthogonality constraint ( or 
O - constraint) , and thus is given by g° : M}'^ — > M: 

^^■^^ = {R^'^a^'\r^'^ + R^'\^^^ - r«) - {a^\ x^'^) = 0. 

Note that an 0-constraint has a singularity if c?'-*'-'-* happens to be zero. Table [1] indi- 
cates how one can define some typical joints using different combinations of the basic 
constraints. 



Table 1. Different types of joints. 



type of joint 


types of constraints 


^ of conditions 


spherical 


1 C 


3 


universal 


1 C and 1 SO 


4 


revolute 


1 C and 2 SO 


5 


cylindrical 


2 SO and 2 


4 


translational 


3 SO and 2 


5 
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6. NUMERICAL RESULTS 

6.1. Implementation. We use a 5th order Runge-Kutta scheme by Dormand and 
Prince In [23] and [2^ we have explained in detail how this scheme (and other 
Runge-Kutta methods) can be adapted to jet space context. To speed up the compu- 
tation system (15. 5p was solved for 4th and 5th order approximations only, not for the 
intermediate points. The tests indicated that this omission did not affect the quality of 
the computed solutions. To solve (15. 3p and (15. 5p we used the inexact Newton method 
[8]; how to apply this in our context is explained in [23]. 

6.2. Description of the models. In the following examples rigid bodies are homoge- 
neous solids with mass density p = 781oE Gravitational acceleration is g = 9.81 in the 
negative direction of X2-axis in spatial coordinates. The basic unit vectors are denoted 
by 

ei:= (1,0,0) 62:= (0,1,0) 63:= (0,0,1). 

In Tables below we will specify the constraints by giving some vectors a(*\ a^^\ b^^^ and 
b^^^ whose interpretation is as follows: 

a*-*-* : orthogonal to axis of a joint in ith. body coordinates 

ft*-*-* : orthogonal to a^^^ and a*--'-* in ith body coordinates 

b^^^ : parallel to fe^*-* in jth body coordinates. 

Given these vectors we will need the following orthogonality constraints: 

SO: g'°{a^'\a^'^), {b^'\ a^^^) , g'°{a^'\b^'^) 

O : g°{a^'\S''^^), g°{b^'\d^''^'>). 

If now in Table [1] we need a certain number of SO- or 0-constraints, then we take 
the corresponding constraints from the above list starting from left. For example the 
universal joint needs just a*^*^ and a^^^ while only the translational joint uses b^^\ 

6.2.1. 3D pendulum. This is a problem of three degrees of freedom. Our pendulum is 
initially at rest along the spatial xi-axis. It is attached to a spherical joint, which lies 
in the spatial origin. There are no external forces acting on the system, except gravity; 
hence the energy W = T + U remains constant. We also tested with an imposed 
constant torque which showed an interesting behaviour and comment briefly on that. 

Initial configuration is shown in Figure 16.11 where the spatial coordinate axes are 
as follows: ei towards the upper right corner, 62 upwards, 63 towards the lower right 
corner. We used the program to calculate the inertial tensor 

1 = diag(0.147, 3.175, 3.154) 

as well as the centre of mass m = 38.34 of the pendulum. Initially the centre of mass in 
spatial coordinates is r = (0.765, 0, O), and the orientation is given by Euler parameters 
9 = (1, 0, 0, 0). The vectors describing the position of the joint are x^^^ = (0, 0, 0) and 
= (-0.765,0,0). 



We use standard SI units in our models. 
' http://www.solidworks.com. 
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Figure 6.1. Initial configuration of tlie pendulum. 



6.2.2. Planar quadrangle with joints. This is a problem of one degree of freedom. Since 
this is a planar problem it is of course inefficient to use a 3D code to solve it. However, 
to model it we use 4 different types of joints, so this problem is very suitable for testing 
basic constraints. The kinematic chain consists of three rigid bodies and a fixed ground 
body. Each body, including the ground one, is attached to an adjacent one through 
a joint. For testing purposes we have chosen revolute joints only at the ground body, 
whereas the other two joints are spherical or cylindrical. There is a single external 
torque acting on one of the bodies. The spatial coordinate axes are in the Figure 16.21 
as follows: ei towards right, 62 upwards, 63 towards the reader. 

In Table m where the corresponding setup is presented, symbol Bi (resp. Bj) stands 
for the "first body" (resp. "second body"). From the same table we find that the joints 
restrict 17 degrees of freedom from the system. From Tables [2] and [3] one can find the 
chosen parameters of the model. 

Initial configuration is sketched in Figure 16.21 where global and local coordinate 
systems are also shown. The local coordinate system of the ground body coincides 
with the global one. The system starts from rest. 
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Table 2. Parameters of the planar quadrangle. 



Body i 


mass 


inertia tensor 


T 


Body 1 


78.10 


1^1^ = diag(0.08, 26.05, 26.1) 


(0, 0, -1200) 


Body 2 


156.20 


I^^^ = diag(0.16, 208.3, 208.4) 




Body 3 


156.20 


1(3) = diag(0.16, 208.3, 208.4) 





Table 3. Initial configuration of the planar quadrangle. 



Body i 




9(^) 


Body 1 


(0.500,0.866,0) 


(0.866,0,0,0.500) 


Body 2 


(2.824,2.553,0) 


(0.978,0,0,0.210) 


Body 3 


(3.574,1.687, 0) 


(0.877,0,0,0.481) 



Table 4. Joints of the planar quadrangle. Here x^^^ is the position of 
the joint in i^^ body coordinate system. 



joint type 


Bi 




# of cond. 






a('0 






revolute 





1 


5 


(0, 0, 0) 


(-1, 0, 0) 


62 


ei 


63 


spherical 


1 


2 


3 


(1, 0, 0) 


(-2, 0, 0) 








cyUndrical 


2 


3 


4 


(2, 0, 0) 


(2, 0, 0) 


ei 




63 


revolute 


3 





5 


(-2, 0, 0) 


(2.5,0,0) 


ei 


62 


63 



Figure 6.2. Initial configuration of the planar quadrangle. 

6.2.3. Crank mechanism. This example is of one degree of freedom, yet a real 3D- 
problem. Again we have three solids, a ground body, and four joints: revolute, spherical, 
universal, and translational. There is an external torque acting on the body 1 so that 
the corner joint is moving along a circle centred at the global origin and perpendicular 
to ei. The spatial coordinate axes are in the Figure 16.31 as follows: ei towards the 
lower right corner, 62 upwards, 63 towards the lower left corner. Joint information is 
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represented in Table [7l Parameters of the model are given in Tables [5] and [6l Initial 
configuration is illustrated in Figure [631 and the system starts from rest. 



Table 5. Parameters of the crank mechanism. 



Body i 


mass 


inertia tensor 


r 


Body 1 


19.50 


1(1) = diag(0.02, 0.41, 0.42) 


(0, 0, -50) 


Body 2 


70.29 


1(2) = diag(0.07, 18.99, 19.04) 




Body 3 


7.81 


I(^) = diag(0.01, 0.01, 0.01) 





Table 6. Initial configuration of the crank mechanism. 



Body i 




0W 


Body 1 


(0,0,-0.25) 


(0.707,0,0.707,0) 


Body 2 


(0.9,0,-0.5) 


(1,0,0,0) 


Body 3 


(1.8,0,-0.5) 


(1,0,0,0) 



Table 7. Joints of the crank mechanism. 



joint type 


I3^ 




# of cond. 






aW 


6(0 






spherical 


1 


2 


3 


(0.25,0,0) 


(-0.9,0,0) 










translat. 





3 


5 


(0, 0, 0) 


(-1.8,0,0.5) 


62 


63 


61 


63 


universal 


2 


3 


4 


(0.9,0,0) 


(0, 0, 0) 


62 




63 




revolute 


1 





5 


(-0.25,0,0) 


(0, 0, 0) 


62 


61 


61 






Figure 6.3. Initial configuration of the crank mechanism. 
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6.3. Test runs. Simulations were done with a 1.75GHz PC using Linux operating 
system, the algorithms were coded entirely in C++ and compiled with the Gnu C++ 
compiler. Figures [6.11 16.21 16.31 were done with OpenGL Framework and other figures 
were plotted with Matlab. In Tables El [9], and [TOl CPU times are given in seconds. 
Time interval was limited to t = [0, 10] in all of the tests and the absolute Newton 
tolerance was taken as tolabs = 10~^. 

We are mainly interested in the following comparisons: 

• The quasi-orthogonal projection presented here (in the Algorithms 15.31 and 15. 4p 
vs. the usual orthogonal projection from |24j . 

• The study of the effect of imposing/ignoring the energy conservation. 

In the absence (resp. presence) of external forces we take f\m = W (resp. f\m = W^totai)- 
Recall that the computation of PVtotai involves a time integration which is handled with 
the trapezoidal rule. 

3D pendulum. In the step size control we use Atol = Rtol = 10~^, faCmax = 3.0 pT] , 
and initial step size = 0.25. The corresponding step sizes are in Figure [631 and run 
characteristics in Table [H 

Compared to our usual orthogonal projection [21], the quasi-orthogonal projection 
needs 10% more timesteps, and rejects steps three times as much as the usual method, 
but uses a bit less Newton iterations (on the average, yet the maximum number is twice 
that of the usual orthogonal projection). Still, the overall CPU time spent by the new 
method is only a half, therefore the quasi-orthogonal approach is 50% faster. This latter 
result illustrates well the noticeable effectiveness of the quasi-orthogonal approach. 
Then again, the number of rejected steps and ij^df\„^, H^d? fmv indicate some sort of 
sensitivity of the quasi-orthogonal method, and it would be useful to find a strategy to 
reduce this sensitivity making the method more robust in this sense. Nevertheless, the 
results were qualitatively correct. 

Also we like to point out that the quasi-orthogonal was more robust than the usual 
orthogonal projection when an external torque r was applied (to save space we do not 
tabulate these computations). When |r| ~ 10, including the energy invariant increased 
the computing time for both methods, but the quasi-orthogonal one suffered much less 
(110% increase) than the orthogonal one (276% increase). When |r| 50 the quasi- 
orthogonal needed a lot more Newton iterations but the stepsize stayed reasonable, 
whereas the orthogonal suffered from radically decreasing stepsize. 

In Table [Tl|, by comparing the "Pendulum" panel to Table [8|, can be seen that 
by ignoring the energy invariant the computation has speeded up in both the quasi- 
orthogonal (20%) and the usual orthogonal projection (5%) as expected due to reduced 
amount of work. But here more interesting is that now the number of quasi-orthogonal 
projections is significantly reduced: it is now about the same as (indeed even less than) 
with the usual projection. This shows that f\m is, while itself a small system, slow- 
ing down the overall convergence of the quasi-orthogonal method. Nevertheless the 
quasi-orthogonal one is 58% faster, the total CPU time is only a third of the usual 
method. 

In Figure 16.41 we have plotted fiuctuations of different energies. Note that we do 
not need to do time integration to compute the total energy, and hence one source of 
numerical errors is eliminated. It is seen that the energy remains practically constant as 



26 



JUKKA TUOMELA, TEIJO ARPONEN, AND VILLESAMULI NORMI 



it should. However, if the conservation of energy is not exphctly imposed, the drift-off 
is quite significant even on short time interval. 

Table 8. Run characteristics of the pendulum. 





quasi-orth. 


orthogonal 




projection 


projection 


# succ. (rej.) steps 


164(36) 


151(12) 


Newton: av(max) 


0.97(8) 


1.43(4) 


# dist 


1395 


1136 


# proj 


1280 


1070 


CPU dist 


0.84 


0.69 


CPU proj 


1.65 


4.30 


CPU total 


2.53 


5.05 




4856 


6124 




9669 


18372 









^ df'mv 


1211 


631 


^ d /inv 


397 


25 




Figure 6.4. On the left: energies of the pendulum when the conserva- 
tion of the energy is imposed. On the right: the total energy with and 
without conservation of energy. 

Planar quadrangle. In the step size control we use Atol = Rtol = 10^®, fac^ax = 3.0, 
and initial step size = 0.25. Run characteristics are in Table [91 The step sizes and 
the corresponding evolution of the energies with the quasi-orthogonal projection are 
represented in Figures 16.71 and 16.61 In the left panel of Figure 16.61 the potential energy 
is oscillating as one would physically expect since it consists only of gravity, moreover 
the period of oscillation is getting shorter as the torque is speeding up the system. This 
speedup is roughly linear in velocity and clearly visible in the quadratic tendency of 
the kinetic energy. 
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2 4 6 8 10 



t[s] 

Figure 6.5. Step sizes of the pendulum. 

On the right panel is a magnification of the total energy which stays nearly constant 
when the conservation of energy is imposed. There are a few occasional spikes, that 
become more frequent and stronger with increasing kinetic energy. The large number 
of maximum Newton iterations is related to these spikes. Without invariants the drift 
off of the energy is again quite rapid. 

The quasi-orthogonal projection needs more steps (twice as many as the usual or- 
thogonal projection) to converge, but is still 50% faster in CPU time. This shows the 
quasi-orthogonal projection is 4 times faster. 

Table 9. Run characteristics of the planar quadrangle. 





quasi-orth. 


orthogonal 




projection 


projection 


# succ. (rej.) steps 


1300(168) 


1218(149) 


Newton: av(max) 


3.25(265) 


1.82(4) 


# dist 


10139 


9427 


# proj 


9721 


10932 


CPU dist 


42.46 


40.95 


CPU proj 


175.66 


457.73 


CPU total 


218.91 


499.41 


i^dg 


47842 


57473 




613105 


977041 






52388 




27958 


14961 


d /inv 


12194 


4746 



Crank mechanism. In the step size control we use Atol = Rtol = 10^^, faCmax = 3.0, 
and initial step size = 0.25. Run characteristics are in Table [TOl Evolution of energies 
is represented in Figure 16.81 and the step sizes in Figure 16. 9[ The results are similar to 
the planar quadrangle case so we will be brief here. The most notable differences are 
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Figure 6.6. On the left: energies of the planar quadrangle when the 
conservation of the energy is imposed. On the right: the total energy 
with and without conservation of energy. 
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Figure 6.7. Step sizes of the planar quadrangle. 



that the differentials of /inv are evaluated 3-5 times as often, and the quasi-orthogonal 
projection needs about the same number of steps yet uses only a quarter of CPU time 
compared to the usual orthogonal projection. Hence the quasi-orthogonal projection is 
4 times faster here as well. 
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quasi-orth. 


orthogonal 




projection 


projection 


^ Slice frei 1 stPDS 


2086f352) 

w KJ W \ ^ / 


2070fll3) 


Newton: av(max) 


6.69(215) 


2.61(6) 


# dist 


16859 


15175 


# proi 


13358 


17458 


CPU dist 


88.08 


78.44 


CPU proj 


757.65 


3198.90 


CPU total 


848.67 


3292.00 




124303 


123597 




1778438 


2101149 






36758 


^ df\n\j 


108843 


31219 


d /inv 


58003 


10981 



crank mechanism crank mechanism 




)i , , , , 1 _3qI , , , , 1 

02468 10 02468 10 
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Figure 6.8. On the left: energies of the crank mechanism when the 
conservation of the energy is imposed. On the right: the total energy 
with and without conservation of energy. 

7. Conclusion and perspectives 

We have derived a computational model to simulate multibody dynamics with holo- 
nomic constraints. This approach is based on jet spaces and Lagrangian formalism and 
avoids the traditional drift-off problems by an orthogonal projection onto the relevant 
manifold. 

As we have seen in the numerical examples above our method can take into account 
arbitrary (holonomic) constraints and in addition we can include any invariants, such 
as the conservation of energy, in our model. Hence our simulations can run indefinitely 
as far as the physical relevance of the constraints is concerned. 

Computationally the most expensive part in our simulations is the projection step; 
this may take as much as 90% of the total time. However, this aspect is not fully 
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Figure 6.9. Step sizes of the crank mechanism. 
Table 1 1 . Test runs without the conservation of energy. 





quasi-orth. 


orthogonal 




projection 


projection 


pendulum: 






# succ. (rej.) steps 


153(23) 


152(13) 


Newton: av(max) 


0.929(2) 


0.987(4) 


# Proj 


795 


833 


CPU total 


1.98 


4.80 




4158 


5885 


planar quadrangle: 






# succ. (rej.) steps 


1487(181) 


1226(161) 


Newton: av(max) 


0.113(2) 


0.995(3) 


# Proj 


4332 


6852 


CPU total 


95.03 


301.05 




32003 


40476 


crank mechanism: 






# succ. (rej.) steps 


1996(87) 


2077(129) 


Newton: av(max) 


0.325(2) 


1.42(5) 


# Proj 


7133 


11147 


CPU total 


191.27 


2099.30 




45054 


87018 



optimized in our code. Much of the time goes to updating various differentials, and it 
is clear that these updates could be less frequent. Another possibility to speed up the 
code would be to use automatic differentiation [TO]. However, exploring this idea was 
left to future work. 

Another topic that we aim to work on is to improve further the quasi-orthogonal 
iteration: now we iterate first onto M^c H M\„y and then onto Mq. However, during the 
latter step 9i may change significantly and we need to re-iterate onto fl M\„y again. 
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One possible reason for this is that in some cases the condition number of the relevant 
matrix in the Newton iteration was relatively big, resulting in the slow convergence. 
This could probably be fixed by some suitable precondition method. In any case it 
would be useful to develop a strategy to get onto the Mq fl Mhc H M\„^ without losing 
the efficiency of the quasi-orthogonal iteration. 

We did not consider nonholonomic systems in the present article because treating 
them would have augmented the length of the paper considerably. The formulation 
of nonholonomic problems is very different from the holonomic ones, for example non- 
holonomic problems are not variational (in a standard sense); see [5l p. 208] for a 
discussion and further references. However, we believe that the general framework of 
jet spaces is also suitable for numerical solution of nonholonomic systems and we hope 
to treat this case in future papers. 

References 

1. F. Amirouche, Fundamentals of multibody dynamics, Birkhauser Boston Inc., Boston, MA, 2006. 

2. P. Appell, Traite de mecanique rationelle, Tome I & Tome II, Editions Jacques Gabay, 1991, 
reimpression de 6'^ ed. publiee par Gauthier-Villars en 1941 (Tome I) et en 1953 (Tome II). 

3. V. I. Arnold, Mathematical methods of classical mechanics, 2nd ed.. Graduate Texts in Mathe- 
matics, vol. 60, Springer- Verlag, New York, 1989. 

4. J. Baumgarte, Stabilization of constraints and integrals of motion in dynamical systems, Comput. 
Methods Appl. Mech. Engrg. 1 (1972), 1-16. 

5. A. Bloch, Nonholonomic mechanics and control. Interdisciplinary Applied Mathematics, vol. 24, 
Springer- Verlag, New York, 2003. 

6. F. BuUo and A. Lewis, Geometric control of mechanical systems, Texts in applied mathematics, 
vol. 49, Springer, 2005. 

7. J. Garcia de Jalon and E. Bayo, Kinematic and dynamic simulation of multibody systems. Me- 
chanical Engineering Series, Springer- Verlag, New York, 1994. 

8. R. Dembo, S. Eisenstat, and T. Steinhaug, Inexact Newton methods, SIAM J. Numer. Anal. 19 
(1982), no. 2, 400-408. 

9. M. Giaquinta and S. Hildebrandt, Calculus of variations I, Grundlehren, vol. 310, Springer, 1996. 

10. A. Griewank, Evaluating derivatives, SIAM, 2000. 

11. E. Hairer, S. N0rsett, and G. Wanner, Solving ordinary differential equations I, Nonstiff problems, 
2nd ed., Springer series in comp. math., vol. 8, Springer, 1993. 

12. O. Krupkova, Mechanical systems with nonholonomic constraints, J. Math. Phys. 38 (1997), no. 10, 
5098-5126. 

13. C. Lanczos, The variational principles of mechanics, 4th ed., Dover, 1970. 

14. J.F. Pommaret, Systems of partial differential equations and Lie pseudogroups, Gordon & Breach, 
London, 1978. 

15. P. Rabier and W. Rheinboldt, Nonholonomic motion of rigid mechanical systems from a DAE 
viewpoint, SIAM, 2000. 

16. D. Saunders, The geometry of jet bundles, London Math. Soc. Lecture note series, vol. 142, Cam- 
bridge university press, 1989. 

17. R. von Schwerin, Multibody system simulation. Lecture Notes in Computational Science and En- 
gineering, vol. 7, Springer- Verlag, Berlin, 1999. 

18. W.M. Seller, Involution — the formal theory of differential equations and its applications in 
computer algebra and numerical analysis, Habilitation thesis. Dept. of Mathematics, Universitat 
Mannheim, 2001, (manuscript accepted for publication by Springer- Verlag) . 

19. A. A. Shabana, Dynamics of multibody systems, 2nd ed., Cambridge University Press, 1998. 

20. M. Spivak, A comprehensive introduction to differential geometry, vol 1-5, 2nd ed., Publish or 
Perish, 1979. 

21. N. N. Tarkhanov, Complexes of differential operators. Mathematics and its Applications, vol. 340, 
Kluwer Academic Publishers Group, Dordrecht, 1995. 



32 



JUKKA TUOMELA, TEIJO ARPONEN, AND VILLESAMULI NORMI 



22. J. Tuomela and T. Arponen, On the numerical solution of involutive ordinary differential systems, 
IMA J. Num. Anal. 20 (2000), 561-599. 

23. , On the numerical solution of involutive ordinary differential systems: Higher order meth- 
ods, BIT 41 (2001), 599-628. 

24. J. Tuomela, T. Arponen, and V. Normi, On the numerical solution of involutive ordinary differ- 
ential systems: Enhanced linear algebra, to appear in IMA J. Num. Anal. 

Department of Mathematics, University of Joensuu, PL 111, 80101 Joensuu, Finland 
E-mail address: jukka.tuoiiiela@joensuu.fi 

Institute of Mathematics, Helsinki University of Technology, PL 1100, 02015 TKK, 
Finland 

E-mail address: teijo.arponen@hut.fi 



Department of Mathematics, University of Joensuu, PL 111, 80101 Joensuu, Finland 
E-mail address: villesamuli.normi@joensuu.fi 



